 /**
 * @file rc_usart.h
 * @author jozenlee (2250017028@qq.com)
 * @brief 
 * @version 0.1
 * @date 2022-03-03
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef _RC_USART_H_
#define _RC_USART_H_

#include <common/Driver/drv_usart.h>
#include <common/DataType/RobotCtrlType.h>

// These data is params of driving HT motors
#define P_MIN -95.5f    // Radians
#define P_MAX 95.5f        
#define V_MIN -45.0f    // Rad/s
#define V_MAX 45.0f 
#define KP_MIN 0.0f     // N-m/rad
#define KP_MAX 500.0f
#define KD_MIN 0.0f     // N-m/rad/s
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))	

#define VERIFY_VALUE    0x88

class RC_Usart
{
public:
      bool stm32InitFlag[2]={false,false};
    RC_Usart(std::string port_name1, std::string port_name2,
        std::int32_t baudrate = 460800, 
        std::int16_t out_time = 100):
        usart1(port_name1,baudrate,  out_time),
        usart2(port_name2,baudrate,  out_time){}
    ~RC_Usart(){}
    void updateData(LegData* data);
    void updateCommand(const RobotCommand* cmd);
    void start(void){ 
        usart1.start();
        usart2.start();
    }
    void stop(void){
        usart1.stop();
        usart2.stop();
    }   
    void updateAU(AU_Type au){
        au_cmd = au;
    }

private:
    Usart usart1, usart2;
    AU_Type au_cmd = AU_NULL;
    // used on the computer
    void Pack_RobotCmd(const RobotCommand* src, uint8_t* dst1, uint8_t* dst2);
    uint8_t Unpack_LegData(const uint8_t* src, LegData* dst);
    
    // used on the STM32
    void Unpack_RobotCmd(uint8_t head, const uint8_t* src, UsartCmdType* dst);
    void Pack_LegData(const UsartDataType* src, uint8_t* dst);

    uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits);
    float uint_to_float(uint16_t x, float x_min, float x_max, uint8_t bits);

};

#endif 